#include "wiimote.h"

wmote::wmote(){
    std::ifstream in("configuration", std::ifstream::in );
    config conf;

    in.read((char*)&conf, sizeof(config));
    in.close();

    num = conf.num_wm;

    if(!connect()) std::cerr << "Error in the connection" << std::endl;
    else{
        set_leds(conf.rumble);
        set_motion_sensing(conf.motion_s);
        if(conf.motion_s){set_orient_threshold(conf.orient_thr);
            set_IR(conf.active_IR, conf.sense);
            if(conf.active_IR) set_IR_opts(conf.aspect, conf.position, conf.x, conf.y);
        }
    }
}

wmote::~wmote(){
    discconnect(SHUTDOWN);
}

status wmote::connect(){
    this->wm = wiiuse_init(this->num);
    int found = wiiuse_find(this->wm, this->num, 5); // 5 seconds
    int connected = wiiuse_connect(this->wm, this->num);

    if(found != connected){
        std::cerr << "An error have ocurred: " << connected << "wiimotes was connected for " << found << std::endl;
        return 0;
    }

    return connected;
}

void wmote::discconnect(int opt){
    if(opt == SHUTDOWN) wiiuse_cleanup(this->wm, this->num);
    else{
        for(int x=0; x< this->num; x++){
            wiiuse_disconnect(this->wm[x]);
        }
    }
}

void wmote::set_leds(int opt){
    int led = 0x10;
    for(int x=0; x<this->num; x++){
        wiiuse_set_leds(this->wm[x], led);
        if(opt == WITH_RUMBLE){
            wiiuse_rumble(this->wm[x], 1);
            #ifndef WIN32
                usleep(200000);
            #else
                Sleep(200);
            #endif
            wiiuse_rumble(this->wm[x], 0);
        }

        led *= 2;
    }
}

void wmote::set_motion_sensing(int opt){
    for(int x=0; x< this->num; x++){
        wiiuse_motion_sensing(this->wm[x], opt);
    }
}

void wmote::set_orient_threshold(float num){
    for(int x=0; x< this->num; x++){
        wiiuse_set_orient_threshold(this->wm[x], num);
    }
}

void wmote::set_IR(int active, int sense){
    for(int x=0; x< this->num; x++){
        if(active){
            if(!WIIUSE_USING_ACC(this->wm[x])) set_motion_sensing(1);
            wiiuse_set_ir(this->wm[x], active);
            wiiuse_set_ir_sensitivity(this->wm[x], sense);
        }
    }
}

void wmote::set_IR_opts(enum aspect_t aspect, enum ir_position_t position, uint x, uint y){
    for(int x=0; x< this->num; x++){
        wiiuse_set_aspect_ratio(this->wm[x], aspect);
        wiiuse_set_ir_position(this->wm[x], position);
        wiiuse_set_ir_vres(this->wm[x], x, y);
    }
}
